
#include "node_sam.h"
#include "serialization/asn1.h"
#include "t1.h"
#include <stdbool.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include <HID/SIM.h>

#include "Debug.h"
#include "Log.h"

int router_queue_push(uint8_t src, uint8_t dest, uint8_t reply, void* payload, uint16_t length);

int router_queue_push_inf(t1_inf_t* inf, uint16_t len);


int node_sam_handler(t1_inf_t *inf, /*t1_len_t*/ uint16_t len) {
	static uint8_t capdu[512];
	static uint8_t rapdu[512];
	uint16_t clen;
	uint16_t rlen;	
    uint8_t inf_header[] = T1_SAM_INF_HEADER_ARR;
	uint16_t ulen = len + sizeof(payload_header_t);
	LOG_I("node sam");
	inf_header[5] = ulen >> 8;
    inf_header[6] = ulen;
    memcpy(capdu, inf_header, sizeof(inf_header));
    memcpy(capdu + sizeof(inf_header), &inf->header, sizeof(payload_header_t));
    memcpy(capdu + sizeof(payload_header_t) + sizeof(inf_header), inf->payload, len);
    clen = sizeof(payload_header_t) + len + 7;
	capdu[clen++] = 0;
	capdu[clen++] = 0;
	rlen = 0;
	
	LOG_SBUF("To SE >", capdu, clen);
	int ret = SIM_Exchange(capdu, clen, rapdu, &rlen);
	if(ret || rlen < 3){
		LOG_E("SE rx fail: %d", ret);
		return -1;
	}
	LOG_SBUF("From SE<", rapdu, rlen);
	payload_header_t *h = (payload_header_t*)rapdu;
	t1_inf_t t1_inf; 
	t1_inf.header = *h;
	t1_inf.payload = rapdu + sizeof(payload_header_t);
	uint16_t payload_length = rlen - sizeof(payload_header_t) - 2;
	router_queue_push_inf(&t1_inf, payload_length);
	return 0;
}

